#include "dog.h"
#include "interrupt_manager.h"
#include "motor_control.h"
#include "remote.h"
#include "fsm.h"
#include "global.h"

/**
 * 狗子初始化
 * 只执行一遍
 */
inline void dog_init(void)
{
    /* 初始化开始 */
    printf("Program start\n");
    /* 初始化遥控 */
    remote_init();
    /* 初始化电机 */
    motor_control_init();
    /* 初始化有限状态机 */
    fsm_init(PASSIVE);
    /* 初始化完成 */
    printf("Initialization complete\n");
}

/**
 * 狗子运行
 * 需要循环执行
 */
inline void dog_run(void)
{
    /* 获得运行时间 ms */
    global.run_time = __HAL_TIM_GetCounter(&GLOBAL_TIM) / 1000.0f;
    /* 获得控制命令 */
    switch (get_cmd())
    {
    case ENTER_PASSIVE:
        fsm_set_state(PASSIVE);
        break;
    case ENTER_FALL:
        fsm_set_state(FALL);
        break;
    case ENTER_FIXSTAND:
        fsm_set_state(FIXEDSTAND);
        break;
    case ENTER_TROTING:
        fsm_set_state(TROTING);
        break;
    };
    /* 运行有限状态机, 执行当前状态函数, 设置电机参数 */
    fsm_update();
    /* 电机运行 */
    motor_control_run();
}
